#include "vehicle_lmpc_factory.hpp"
#include "test_vehicle_mpc_path.h"
using namespace auto_ros::control;
int main()
{
	std::string dyn_yaml_path_name = test_vehicle_mpc_path + "mpc_ll_4d4s/";
	std::string vehicle_lmpc_yaml_file = "vehicle_lmpc.yaml";
	VehicleLMpcFactory test_lmpc_factory;
	std::shared_ptr<VehicleLMpcBase> test_mpc =
		test_lmpc_factory.CreateSharedObject("VehicleLL4d4sMpc",
											 dyn_yaml_path_name, vehicle_lmpc_yaml_file, 50);
	//std::cout << test_mpc->vehicle_dyn_ptr_->dyn_yaml_ << std::endl;
	//bool VehicleLLMpc::step(const Eigen::VectorXd &x0,
	//					const Eigen::VectorXd &u_fb,
	//					const PlanningRefPoint &ref_point,
	//					double speed)
	Eigen::VectorXd x0(test_mpc->vehicle_dyn_ptr_->state_dim());
	x0 << 1.0, 0.0, 0.0, 0.1, 0.0;
	Eigen::VectorXd u_fb(test_mpc->vehicle_dyn_ptr_->control_dim());
	u_fb << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
	PlanningRefPoint ref_point;
	ref_point.x_ = 0.0;
	ref_point.y_ = 0.0;
	ref_point.theta_ = 0.0;
	ref_point.kappa_ = 0.01;
	ref_point.v_ = 12.0;
	ref_point.s_ = 0.0;
	ref_point.a_ = 0.5;
	double speed = 10.0;
	std::map<std::string, double> other_input = {{"ax", 1.0}, {"ay", 0.0}};
	test_mpc->step(x0,
				   u_fb,
				   ref_point,
				   speed, other_input);
	//std::cout << test_kf->state_esti() << std::endl;
}